package com.crunch.physics;

import com.crunch.math.Linef;
import com.crunch.math.MathOps;
import com.crunch.math.Vector2f;

import java.util.ArrayList;
import java.util.List;

/**
 * Manages contacts between objects in simulation.
 */
class ContactManager {
	ContactManager(PhysicsSimulation simulation) {
		this.simulation = simulation;

		queryBuffer = new ArrayList<HashGrid.Entity>();

		movementContacts = new ArrayList<MovementContact>();
		uniqueMovementContacts = new ArrayList<MovementContact>();
		freeMovementContacts = new ArrayList<MovementContact>();

		contactEdges = new ArrayList<ContactEdge>();
		freeContactEdges = new ArrayList<ContactEdge>();

		contactLines = new ArrayList<Linef>();
		freeContactLines = new ArrayList<Linef>();
	}

	// updates contacts for a single entity
	public void updateContacts(DynamicPhysicsEntity e) {
		// first, free all contacts in dynamic entity
		e.floorContact = null;
		while (e.contactList != null) {
			Contact next = e.contactList.getNext();
			destroyContact(e.contactList);
			e.contactList = next;
		}

		// if not in the simulation, return immediately
		if (!e.checkFlag(DynamicPhysicsEntity.FLAG_IN_SIMULATION)) {
			return;
		}

		determineContacts(e, 0.0f);
		e.floorContact = null;
		int i = 0;
		for (MovementContact mc : movementContacts) {
			Contact c = createContact();
			c.reset(mc.type, mc.object, mc.index, mc.point, mc.normal, mc.velocity, mc.unique);
			c.next = e.contactList;
			e.contactList = c;

			if (floorContactIndex == i) {
				e.floorContact = c;
			}

			++i;
		}

		clearMovementContacts();
	}

	public void determineContacts(DynamicPhysicsEntity d, float t) {
		// query scene to determine which edges are nearby
		Vector2f aabbMin = new Vector2f();
		Vector2f aabbMax = new Vector2f();
		d.getAabb(aabbMin, aabbMax, simulation.settings.contactThreshold);

		// query grid for intersections
		HashGrid.CollisionAabbSet set = simulation.hashGrid.getCollisionAabbSet(
				(1 << PhysicsSimulation.GRID_STATIC_COLLIDER_TYPE) | (1 << PhysicsSimulation.GRID_MOVABLE_ENTITY_TYPE),
				aabbMin, aabbMax);
		queryBuffer.clear();
		for (HashGrid.Entity e : set) {
			queryBuffer.add(e);
		}
		Helpers.sortAndRemoveDuplicates(queryBuffer);

		Vector2f movableEntityOffset = new Vector2f();

		// add contacts
		for (HashGrid.Entity e : queryBuffer) {
			int type = e.getType();
			if (type == PhysicsSimulation.GRID_STATIC_COLLIDER_TYPE) {
				// add single contact for static collider
				Segment segment = simulation.staticColliders.get((Integer) e.getData());
				ContactEdge ce = createContactEdge();
				ce.type = Contact.Type.STATIC_COLLIDER;
				ce.object = null;
				ce.index = (Integer) e.getData();
				ce.normal.set(segment.getNormalDirect());
				ce.vertices[0].set(segment.getVerticesDirect()[0]);
				ce.vertices[1].set(segment.getVerticesDirect()[1]);
				contactEdges.add(ce);
			} else if (type == PhysicsSimulation.GRID_MOVABLE_ENTITY_TYPE) {
				// add contacts for each edge in movable entity
				MovablePhysicsEntity movableEntity = (MovablePhysicsEntity) e.getData();
				// offset the movable entity into current timestep by velocity*t
				movableEntityOffset.set(movableEntity.getVelocity());
				movableEntityOffset.multiplyAssign(t);
				Polygon shape = movableEntity.getShape();
				for (int i = 0; i < shape.getVertexCount(); ++i) {
					int i1 = (i+1 == shape.getVertexCount()) ? 0 : i+1;
					ContactEdge ce = createContactEdge();
					ce.type = Contact.Type.MOVABLE_ENTITY;
					ce.object = e.getData();
					ce.index = i;

					// transform vertices and normal
					ce.vertices[0].set(shape.getVertexDirect(i));
					ce.vertices[1].set(shape.getVertexDirect(i1));
					Helpers.transformDirect(ce.vertices[0], movableEntity.rotationVector, movableEntity.position);
					Helpers.transformDirect(ce.vertices[1], movableEntity.rotationVector, movableEntity.position);
					ce.normal.set(shape.getNormalDirect(i));
					Helpers.transformDirect(ce.normal, movableEntity.rotationVector, null);
					ce.vertices[0].addAssign(movableEntityOffset);
					ce.vertices[1].addAssign(movableEntityOffset);

					contactEdges.add(ce);
				}
			}
		}

		// find which contacts are valid
		findValidContacts(d);

		// determine floor contact - contact with largest dot with floorNormal
		floorContactIndex = -1;
		if (d.sticksToFloor) {
			float bestDot = Float.NEGATIVE_INFINITY;
			for (int i = 0; i < movementContacts.size(); ++i) {
				MovementContact mc = movementContacts.get(i);
				if (mc.unique) {
					float dot = MathOps.dot(mc.normal, d.floorNormal);
					if (dot >= d.minNDotFloorNormal) {
						// find the least sloped contact - the one with the greatest dot
						if (dot > bestDot) {
							bestDot = dot;
							floorContactIndex = i;
						}
					}
				}
			}
		}
	}

	public List<MovementContact> getMovementContacts() {
		return movementContacts;
	}

	public List<MovementContact> getUniqueMovementContacts() {
		return uniqueMovementContacts;
	}

	public void clearMovementContacts() {
		for (MovementContact mc : movementContacts) {
			destroyMovementContact(mc);
		}
		movementContacts.clear();
		uniqueMovementContacts.clear();
		floorContactIndex = -1;
	}

	// fills movementContacts with valid contacts
	// contactEdges and contactLines are freed before this method terminates
	private void findValidContacts(DynamicPhysicsEntity d) {
		// we start with a list of contacts which may or may not be valid
		// i.e. some of them might be out of range
		// we want to construct a list of valid contacts and identify duplicates

		// first clear current valid constraints
		for (MovementContact mc : movementContacts) {
			destroyMovementContact(mc);
		}
		movementContacts.clear();
		uniqueMovementContacts.clear();

		// allocate a DistanceResult for repeated use
		PhysicsMath.DistanceResult dist = new PhysicsMath.DistanceResult();
		dist.pointA = new Vector2f();
		dist.vectorAB = new Vector2f();

		// we first make a pass through all contacts and eliminate ones which are not in valid range
		for (int i = contactEdges.size()-1; i >= 0; --i) {
			ContactEdge ce = contactEdges.get(i);
			// todo: we can add a function which does this more optimally in a single step
			PhysicsMath.distanceCapsuleSegmentLine(d.shape, d.upVector, d.position, ce.vertices, ce.normal, dist);
			float contactDist = dist.distance;
			if (Float.isInfinite(contactDist)) {
				PhysicsMath.distanceCapsuleSegmentVertex(d.shape, d.upVector, d.position, ce.vertices[0], dist);
				float dA = dist.distance;
				PhysicsMath.distanceCapsuleSegmentVertex(d.shape, d.upVector, d.position, ce.vertices[1], dist);
				float dB = dist.distance;
				contactDist = Math.min(dA, dB);
			}
			boolean inRange = contactDist < simulation.settings.contactThreshold;
			if (!inRange) {
				// move back to this slot and pop back
				contactEdges.set(i, contactEdges.get(contactEdges.size()-1));
				contactEdges.remove(contactEdges.size()-1);
				destroyContactEdge(ce);
			}
		}

		// now we have a list of contact edges which are all within a valid range
		// we now determine which of these are valid and unique
		// we still want to add all of them to the final list (so even invalid ones can be ignored when constraining velocity)
		// however, we flag these ones as "invalid"

		// no movement contacts created yet
		for (ContactEdge ce : contactEdges) {
			ce.hasMovementContact = false;
		}

		// 1) find contacts for each edge
		// 2) find contacts for each vertex not contained in an edge

		// first test each line
		for (ContactEdge ce : contactEdges) {
			PhysicsMath.distanceCapsuleSegmentLine(d.shape, d.upVector, d.position, ce.vertices, ce.normal, dist);
			if (dist.distance < simulation.settings.contactThreshold) {
				// add a movement contact
				MovementContact mc = createMovementContact();
				mc.type = ce.type;
				mc.object = ce.object;
				mc.index = ce.index;
				mc.point.set(dist.pointA);
				mc.normal.set(dist.vectorAB);
				mc.normal.negateAssign();
				movementContacts.add(mc);
				// this edge caused a movement contact
				ce.hasMovementContact = true;

				// add a contact line to test vertices against
				Linef line = createContactLine();
				line.setPointNormal(ce.vertices[0], ce.normal);
				contactLines.add(line);
			}
		}

		// next test each vertex
		final float LINE_THRESHOLD = 0.0001f;
		for (ContactEdge ce : contactEdges) {
			for (int v = 0; v < 2; ++v) {
				Vector2f vertex = ce.vertices[v];
				// make sure vertex doesn't lie on a line
				boolean inLine = false;
				for (int l = 0; !inLine && l < contactLines.size(); ++l) {
					Linef line = contactLines.get(l);
					if (line.distance(vertex) < LINE_THRESHOLD) {
						inLine = true;
					}
				}
				if (inLine) {
					continue;
				}

				PhysicsMath.distanceCapsuleSegmentVertex(d.shape, d.upVector, d.position, vertex, dist);
				if (dist.distance < simulation.settings.contactThreshold) {
					// add a movement contact
					MovementContact mc = createMovementContact();
					mc.type = ce.type;
					mc.object = ce.object;
					mc.index = ce.index;
					mc.point.set(dist.pointA);
					mc.normal.set(dist.vectorAB);
					mc.normal.negateAssign();
					movementContacts.add(mc);
					// this vertex's edge caused a movement contact
					ce.hasMovementContact = true;
				}
			}
		}

		// set up velocity of each movement contact
		for (MovementContact mc : movementContacts) {
			if (mc.type == Contact.Type.STATIC_COLLIDER) {
				mc.velocity.setZero();
			} else if (mc.type == Contact.Type.MOVABLE_ENTITY) {
				MovablePhysicsEntity o = (MovablePhysicsEntity) mc.object;
				mc.velocity.set(o.velocity);
			} else {
				assert false;
			}
		}

		// determine unique constraints
		final float NORMAL_DOT_THRESHOLD = 0.999f;
		final float VELOCITY_THRESHOLD = 0.001f;
		for (MovementContact mc : movementContacts) {
			mc.unique = true;
			// all contacts in this list are valid for constraining velocity (but they may still be ignored if not unique)
			mc.velocityConstraint = true;
			// test against each unique contact found so far
			for (int t = 0; mc.unique && t < uniqueMovementContacts.size(); ++t) {
				MovementContact umc = uniqueMovementContacts.get(t);
				// dot with unique contact's normal
				// if the velocity and normals are too similar, the contact is not unique
				float velDiff2 = MathOps.distanceSquared(mc.velocity, umc.velocity);
				float dot = MathOps.dot(mc.normal, umc.normal);
				mc.unique = (velDiff2 > VELOCITY_THRESHOLD*VELOCITY_THRESHOLD) || (dot < NORMAL_DOT_THRESHOLD);
			}
			if (mc.unique) {
				uniqueMovementContacts.add(mc);
			}
		}

		// finally, add a movement contact for each edge in range which did not already create movement contacts
		// however, flag them as "invalid"
		// this is so these edges don't affect constrained velocity computation, but will still be ignored after constrained velocity is computed
		for (ContactEdge ce : contactEdges) {
			if (!ce.hasMovementContact) {
				MovementContact mc = createMovementContact();
				mc.type = ce.type;
				mc.object = ce.object;
				mc.index = ce.index;
				mc.unique = false;
				// this contact should NOT constrain velocity
				mc.velocityConstraint = false;
				movementContacts.add(mc);
			}
		}

		// free intermediate lists
		for (ContactEdge ce : contactEdges) {
			destroyContactEdge(ce);
		}
		contactEdges.clear();
		for (Linef l : contactLines) {
			destroyContactLine(l);
		}
		contactLines.clear();
	}

	public void destroyContactList(Contact contactList) {
		while (contactList != null) {
			Contact next = contactList.next;
			destroyContact(contactList);
			contactList = next;
		}
	}

	private Contact createContact() {
		Contact c = freeContactList;
		if (c == null) {
			c = new Contact();
		} else {
			freeContactList = c.next;
			c.next = null;
		}
		return c;
	}

	private void destroyContact(Contact c) {
		c.object = null;
		c.next = freeContactList;
	}

	private Contact freeContactList;

	// used to determine scene contacts

	// contact used to constrain movement
	public static class MovementContact {
		public Contact.Type type;                           // type of object this contact belongs to
		public Object object;                               // object this contact belongs to (null if static collider)
		public int index;                                   // index of collider within object
		public Vector2f point = new Vector2f();             // point of contact
		public Vector2f normal = new Vector2f();            // normal vector of this contact
		public Vector2f velocity = new Vector2f();          // velocity of this contact (e.g. for a movable entity)
		public Vector2f relativeVelocity = new Vector2f();  // relative velocity of this contact to the entity
		public boolean unique;                              // whether this contact is unique
		public boolean velocityConstraint;                  // whether this contact should be used to constrain velocity
	}

	// a contact against the "edge" portion of a collider
	private static class ContactEdge {
		public Contact.Type type;
		public Object object;
		public int index;
		public Vector2f normal = new Vector2f();
		public Vector2f[] vertices = new Vector2f[] { new Vector2f(), new Vector2f() };
		public boolean hasMovementContact; // whether this contact edge has caused a movement contact to be created
	}

	private ArrayList<HashGrid.Entity> queryBuffer;
	private ArrayList<MovementContact> movementContacts;        // list of contacts which constrain movement
	private ArrayList<MovementContact> uniqueMovementContacts;  // list of unique movement contacts
	private ArrayList<ContactEdge> contactEdges;                // list of contacts against edges
	private ArrayList<Linef> contactLines;                      // lines in the contacts list

	private int floorContactIndex;                              // index of floor contact in movement contacts, or -1

	private ArrayList<MovementContact> freeMovementContacts;
	private MovementContact createMovementContact() {
		return freeMovementContacts.isEmpty() ?
				new MovementContact() : freeMovementContacts.remove(freeMovementContacts.size()-1);
	}
	private void destroyMovementContact(MovementContact mc) {
		freeMovementContacts.add(mc);
	}

	private ArrayList<ContactEdge> freeContactEdges;
	private ContactEdge createContactEdge() {
		return freeContactEdges.isEmpty() ?
				new ContactEdge() : freeContactEdges.remove(freeContactEdges.size()-1);
	}
	private void destroyContactEdge(ContactEdge ce) {
		freeContactEdges.add(ce);
	}

	private ArrayList<Linef> freeContactLines;
	private Linef createContactLine() {
		return freeContactLines.isEmpty() ?
				new Linef(new Vector2f(), 0.0f) : freeContactLines.remove(freeContactLines.size()-1);
	}
	private void destroyContactLine(Linef l) {
		freeContactLines.add(l);
	}

	private PhysicsSimulation simulation;
}
